from abstract_sensor.sensor_interfaces import SimulatedSensor
from auxillaries.periodic_task import PeriodicTask
from auxillaries.check_observability import CheckObservability
import time
import random
import math
import quantities

class MySimulatedCamera(SimulatedSensor):
    
    def __init__(self, sensorDataSheet, position, orientation, logger=[]):
        SimulatedSensor.__init__(self,
                                 sensorDataSheet=sensorDataSheet,
                                 position=position,
                                 orientation=orientation,
                                 logger=logger)
        self.mySystem=MySystem()

    def aggregateMeasurements(self):
        self.sim_position=self.mySystem.getPosition()
        inside=CheckObservability.sim_test(sensorPos=self.position, 
                                    sensorOri=self.orientation,
                                    beam=self.properties.getBeamMinMax(),
                                    measurement=self.sim_position)
        noise_level=20
        self.measurement=[0,0]
        if inside==True:
            self.measurement[0]=self.sim_position[0]+(noise_level*random.random()-noise_level/2)
            self.measurement[1]=self.sim_position[1]+(noise_level*random.random()-noise_level/2)
        return self.measurement

class MySimulatedIRSensor(SimulatedSensor):
    
    def __init__(self, sensorDataSheet, position, orientation, logger=[]):
        SimulatedSensor.__init__(self,
                                 sensorDataSheet=sensorDataSheet,
                                 position=position,
                                 orientation=orientation,
                                 logger=logger)
        self.mySystem=MySystem()


    def aggregateMeasurements(self):
        self.sim_position=self.mySystem.getPosition()
        inside=CheckObservability.sim_test(sensorPos=self.position, 
                                    sensorOri=self.orientation,
                                    beam=self.properties.getBeamMinMax(),
                                    measurement=self.sim_position)
        noise_level=2
        self.measurement=[0]     
        if inside==True:
            self.measurement[0]=math.sqrt(math.pow(self.sim_position[0]-self.position[0],2) + math.pow(self.sim_position[1]-self.position[1],2) ) + \
                                (noise_level*random.random()-noise_level/2)
        return self.measurement*quantities.cm 
    
class MySimulatedTemp(SimulatedSensor):
    
    def __init__(self, sensorDataSheet, logger=[]):
        SimulatedSensor.__init__(self,
                                 sensorDataSheet=sensorDataSheet,
                                 position=[],
                                 orientation=[],
                                 logger=logger)
        self.mySystem=MySystem()

    def aggregateMeasurements(self):
        return 20*quantities.degree 


class MySystem():
    @classmethod
    def getPosition(self):
        t=time.time()
        angleDegree=3*(t-round(t/100)*100)           # = 360 Grad in 60 sec
        angleDegree=6*angleDegree                   # = accerleration
        angleRad=angleDegree/180*math.pi
        radius=100
        state=[radius*math.cos(angleRad),
               radius*math.sin(angleRad),
               5]
        return state
    
    @classmethod
    def getOrientation(self):
        t=time.time()
        angleDegree=3*(t-round(t/100)*100)           # = 360 Grad in 60 sec
        angleDegree=6*angleDegree                   # = accerleration
        return angleDegree+90    